/************************************************

     Brian Bousquet-Smith     Chaz Godwin
                
            OBDII Reader
                 Fall 2011
************************************************/

#include <avr/io.h>
#include <avr/iom164.h> 
#include <avr/interrupt.h>
#include <avr/delay.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>


/************************************************
             Defines
************************************************/

// States

#define FCPU        1000000UL
#define OK         0
#define ELM_INIT_STATE    1
#define LCD_INIT_STATE    2
#define    OBD_INIT_STATE    3
#define DASHBOARD    4
#define PERFORMANCE    5
#define DIAGNOSTICS    6
#define ABOUT        7

// ELM Defines

#define COMMAND_END        0x0D

// OBD Defines

#define RESPONSE_END        0x3E



/************************************************
        Function Prototypes
************************************************/

// Initializing Functions

void power_on();
void serial_init();
int elm_init();
int obd_init();
int get_mph();

// Tx Rx Functions

void lcd_transmit(unsigned char data);
void obd_transmit(unsigned char data);
unsigned char lcd_receive();
unsigned char obd_receive();
void lcd_flush();
void obd_flush();
int obd_sendcmd(unsigned char *in, unsigned char *out, char len_in);
int lcd_sendcmd(unsigned char *in, char len_in);

// Debugging Functions

void print_bits(char byte);
void newline (void);

/************************************************
        Global Variables
************************************************/

int state = ELM_INIT_STATE;
int timeoutflag = 0;

/************************************************
        Main Code
************************************************/




int main(void){

/*
*    Purpose: Output RPM and MPH data from a vehicle
*/
    power_on();
    serial_init();
    obd_flush();
    lcd_flush();
    int response = -1;
    _delay_ms(5000);
    while(1){
        PORTA = state;
        switch(state){
        case ELM_INIT_STATE: // Initializes ELM
            response = elm_init();
            if(response == OK){
                state = OBD_INIT_STATE;
                break;
            }
            else{

                break; 
            }

        case LCD_INIT_STATE: // Initializes LCD
            //response = lcd_init();
            if(response == OK){
                state = OBD_INIT_STATE;
                break;
            }
            else break;
    
        case OBD_INIT_STATE: // Initializes OBDII
            response = obd_init();
            if(response == OK){
                state = DASHBOARD;
                break;
            }
            else{
                PORTA = 0x07; 
                break;
            }

        case DASHBOARD: // Virtual Dashboard
            response = get_mph();
            //response = get_rpm();
            _delay_ms(50);
            break;
        
        case PERFORMANCE:
            break;        
        
        case DIAGNOSTICS:
            break;
        
        case ABOUT:
            break;
        } 
    }
    return 0;
}

void power_on(){

/*
*    Purpose: Turn the power light on of the device
*    Input: Nothing
*    Output: Nothing
*/

    DDRA=0xFF;
    PORTA = 0x0F;

}

void serial_init(){

/*
*    Purpose: Initialize Serial on Both UART Ports on the AVR
*    Input: Nothing
*    Output: Nothing
*/

    UCSR0A |= (1<<U2X0);            // Double speed for more precision
    UCSR0B |= (1<<RXEN0)|(1<<TXEN0);    // Allow transmit and receive
    UCSR0C |= (0<<USBS0)|(3<<UCSZ00);     // 8 Bit No Parity 1 Stop Bit
    UBRR0H = 0;                // Sets Baud Rate = 9600
    UBRR0L = 12;

    UCSR1A |= (1<<U2X1);            // Double speed for more precision
    UCSR1B |= (1<<RXEN1)|(1<<TXEN1);     // Allow transmit and receive
    UCSR1C |= (1<<USBS1)|(3<<UCSZ10);     // 8 Bit 2 Stop Bits
    UBRR1H = 0;                // Sets Baud Rate = 9600
    UBRR1L = 12;
}





int elm_init(){

/*
*    Purpose: Initialize ELM and check that communication is correct
*    Input: Nothing
*    Output:    Error Codes:
*        0 -    Successful
*        -1 -     Unsuccessful
*/
    int error = 0;    
    unsigned char error_buff[15];
    unsigned char in_buff[3] = {'A', 'T', 'Z'};
    int i;        
    unsigned char exp_buff[16] = {'A', 'T', 'Z', 'E', 'L', 'M', '3', '2', '7', ' ', 'v', '1', '.', '4', 'b', 0x3E};
    unsigned char out_buff[100];
    unsigned char print_buff[9];    
    int size;
    size = obd_sendcmd(in_buff, out_buff, 3);    
    if(size < 0){
        sprintf(error_buff, "Error OBD CMD \n");
        lcd_sendcmd(error_buff, sizeof(error_buff));
    }    
    int j = 0;;
    for(i=0;i<sizeof(exp_buff);i++){    
        while( (out_buff[j]=='\n') || (out_buff[j]=='\r') ) j++;
        if(out_buff[j]!=exp_buff[i]){
            sprintf(error_buff, "Error ELM Init\n");
            lcd_sendcmd(error_buff, sizeof(error_buff));
            lcd_transmit(i+48);
            return -1;
        }
        j++;
        
    }    
    sprintf(print_buff, "Welcome!\n");
    lcd_sendcmd(print_buff, sizeof(print_buff));
    return 0;
    
}
    
//int obd_init(){

/*
*    Purpose: Initialize OBDII port and check that communication is correct
*    Input: Nothing
*    Output: Error Codes:
*        0 -    Successful
*        -1 -     Error Talking to ELM/OBD
*        -2 -     Still Searching
*        -3 -     Data wrong
*/
    
/*    unsigned char in_buff[4] = {'0', '1', '0', '0'};
    unsigned char exp_buff[11] = {'0', '1', '0', '0', 0x41, 0x00, 0, 0, 0, 0, 0x3E};
    unsigned char out_buff[10]; 
    unsigned char error_buff[15];
    int i;    
    int error = 0;
    unsigned char response;

    error = obd_sendcmd(in_buff, out_buff, 4, 7);
    if(error != 0){
        sprintf(error_buff, "Error OBD CMD \n");
        lcd_sendcmd(error_buff, sizeof(error_buff));
        return -1;
    }
    for(i=0;i<sizeof(out_buff);i++){
        //print_bits(out_buff[i]);
        //lcd_transmit('\n');
        //lcd_transmit('\r');
        if(!(i==6||i==7||i==8||i==9)){
            if(out_buff[i]!=exp_buff[i]){
                if(out_buff[i] == 'S'){
                    sprintf(error_buff, "Finding Prtcol\n");
                    return -2;
                }
                print_bits(out_buff[i]);
                sprintf(error_buff, "Err Bad Data%d\n\r", i);
                lcd_sendcmd(error_buff, sizeof(error_buff));
                return -3;
            }
        }
    }    */
    
int obd_init(){

/*
*    Purpose: Initialize OBDII port and check that communication is correct
*    Input: Nothing
*    Output: Error Codes:
*        0 -    Successful
*        -1 -     Unsuccessful
*/
    
    unsigned char init_buffer[4] = {'0', '1', '0', '0'};
    unsigned char obd_buffer[] = "SEARCHING...";
    int i;
    int flag;
    int count;    
    unsigned char echo;
    unsigned char response;
    
    // Figure out if OBD has valid protocol/is connected
    
    for(i=0;i<4;i++){
        obd_transmit(init_buffer[i]);
        echo = obd_receive();
        if (echo != init_buffer[i]) return -1;
    }
    obd_transmit(COMMAND_END);
    obd_receive(); // Get this echo
    obd_receive(); // Get new line feed     
    
    /* At this point OBD should response with 
        Searching...
           If nothing found outputs
            Unable to Connect
        If found protocol
            41 00 + 4 Bytes of Data 
    */
    flag = 0;
    count = 0;
        
    while( (response = obd_receive()) != RESPONSE_END){
        if(response == 'U') flag = -1;        
        lcd_transmit(response);
    }
    return flag;

}
//}

int get_mph(void){

/*
*    Purpose: Get MPH Data and Output it
*    Input: Nothing
*    Output: Error Codes:
*        0 - Successful
*        -1 - Unsuccessful
*/
    int i, j=0;
    unsigned int mph;
    unsigned char error_buff[15];
    unsigned char in_buff[4] = {'0', '1', '0', 'D'};    
    unsigned char exp_buff[8] = {'0', '1', '0', 'D', '4', '1', '0', 'D'};     
    unsigned char out_buff[100];
    unsigned char mph_buff[2]; 

    int size = 0;
    size = obd_sendcmd(in_buff, out_buff, 4);
    for(i=0;i<sizeof(exp_buff);i++){    
        while( (out_buff[j]=='\n') || (out_buff[j]=='\r') || (out_buff[j]==' ')) j++;
        if(out_buff[j]!=exp_buff[i]){
            sprintf(error_buff, "Error MPH Data\n");
            lcd_sendcmd(error_buff, sizeof(error_buff));
            lcd_transmit(i+48);
            return -1;
        }
        j++;    
    }    
    i = 0;
    int count;
    while(out_buff[j] != 0x3E){
        while( (out_buff[j]=='\n') || (out_buff[j]=='\r') || (out_buff[j]==' ')) j++;        
        if(out_buff[j] == 0x3E) break;
        print_bits(out_buff[j]);
        newline();        
        mph_buff[i] = out_buff[j];
        j++;
    }
    sscanf(mph_buff, "%x%x", &mph);
    mph = mph*.621371;    // Converts RPM to MPH
    if(mph>99) size = 9;
    else if(mph<10) size = 7;
    else size = 8;
    unsigned char print_buff[size];
    sprintf(print_buff, "MPH: %u", mph);
    lcd_sendcmd(print_buff, sizeof(print_buff));
    return 0;
}

int get_rpm(void){

/*
*    Purpose: Get RPM Data and Output it
*    Input: Nothing
*    Output: Error Codes:
*        0 - Successful
*        -1 - Unsuccessful
*/

    int i, j=0;
    unsigned int rpm;
    unsigned char error_buff[15];
    unsigned char in_buff[4] = {'0', '1', '0', 'C'};    
    unsigned char exp_buff[8] = {'0', '1', '0', 'C', '4', '1', '0', 'C'};     
    unsigned char out_buff[100];
    unsigned char rpm_buff[4]; 
    char print_buff[10] ;
    int size = 0;
    size = obd_sendcmd(in_buff, out_buff, 4);
    for(i=0;i<sizeof(exp_buff);i++){    
        while( (out_buff[j]=='\n') || (out_buff[j]=='\r') || (out_buff[j]==' ')) j++;
        if(out_buff[j]!=exp_buff[i]){
            sprintf(error_buff, "Error RPM Data\n");
            lcd_sendcmd(error_buff, sizeof(error_buff));
            lcd_transmit(i+48);
            return -1;
        }
        j++;    
    }    
    i = 0;
    while(out_buff[j] != 0x3E){
        while( (out_buff[j]=='\n') || (out_buff[j]=='\r') || (out_buff[j]==' ')) j++;
        if(out_buff[j] == 0x3E) break;
        print_bits(out_buff[j]);
        rpm_buff[i] = out_buff[j];
        j++;
    }
    rpm = atoi(rpm_buff);
    rpm = rpm/4;
    sprintf(print_buff, "RPM: %u", rpm);
    lcd_sendcmd(print_buff, sizeof(print_buff));
    return 0;
}

int lcd_sendcmd(unsigned char *in, char len_in){

/*
*    Purpose: Send a string to the LCD to print
*    Input:    in - Input buffer to be printed
*        len-in - Length of the buffer to be printed
*    Output: Error Codes:
*        0 - Successful
*        -1 - Unsuccessful
*/    


    int i;
    //unsigned char input =
    for(i=0;i<len_in;i++){
        lcd_transmit(in[i]);        
    }    
    lcd_transmit('\n');
    lcd_transmit('\r');
    return 0;
}

int obd_sendcmd(unsigned char *in, unsigned char *out, char len_in){
    // Len Out is the lenght of the string out, can't exceed 100 bytes    


    //TODO Start Interrupt
    int i;
    obd_flush();    
    for(i=0;i<len_in;i++){
        obd_transmit(in[i]);
    }
    obd_transmit(0x0D);
    for(i=0;i<100;i++){
        out[i] = obd_receive();
        if (out[i] == 0x3E){
            return i;
        }
    }
    return -1;
}

/*void int_timer(void){
    TCCR1B |= (1<<WGM12);
    TIMSK |= (1 << OCIE1A);
    sei();
    OCR1A = 31249;
    TCR1B |= (1<<CS11)|(1<<CS10);
}

ISR(*/

void lcd_transmit (unsigned char data){
    
/*
*    Purpose: Transmit data to the LCD
*    Input: Data to be transmitted
*    Output:    Nothing
*/
    while( !(UCSR1A & (1<<UDRE1)));
    UDR1 = data;
}

unsigned char lcd_receive (void){

/*
*    Purpose: Receive data from the LCD
*    Input: Nothing
*    Output:    Data received    
*/

    while (!(UCSR1A & (1<<RXC1)));
    return UDR1;
}    

void lcd_flush(void){
    char tmp;
    while (UCSR1A & (1<<RXC1) ) tmp = UDR1;
}

void obd_transmit (unsigned char data){

/*
*    Purpose: Transmit data to the LCD
*    Input: Data to be transmitted
*    Output:    Nothing
*/

    while( !(UCSR0A & (1<<UDRE0)));
    UDR0 = data;
}

unsigned char obd_receive (void){

/*
*    Purpose: Receive data from the LCD
*    Input: Nothing
*    Output:    Data received    
*/
    while (!(UCSR0A & (1<<RXC0)));
        
    return UDR0;
}

void obd_flush(void) {
    char tmp;
    while( UCSR0A & (1<<RXC1)) tmp = UDR0;
}

/*char obd_sendcmd (char* in, char* out, char len_in, len_out){    
    //START CLOCK
    int i;    
    for(i=0;i<len_in;i++){
        obd_transmit(in+i);
    }
    for(i=0;i<len_out;i++){


}*/    

//char lcd_sendcmd (char* in, char* out, char len_in, len_out){

//}

void print_bits( char byte){
    char mask[8] = {
    0b10000000,
    0b01000000,
    0b00100000,
    0b00010000,
    0b00001000,
    0b00000100,
    0b00000010,
    0b00000001};

    int m;
    char tmp;
    for(m=0;m<8;m++){
        tmp = ((mask[m]&byte)>>(7-m))+48;
        lcd_transmit(tmp);
    }
}        

void newline(){
    lcd_transmit('\n');
    lcd_transmit('\r');
}







    
 